﻿using System;
using Microsoft.Kinect;
using System.Diagnostics;

namespace AMEE.PlaneDetection.Kinect
{
    /// <summary>
    /// Point Cloud Argument of a PointCloud Ready Event.
    /// </summary>
    public class PointCloudReadyEventArg : EventArgs {
        /// <summary>
        /// Gets or sets the reference(!) to point cloud.
        /// </summary>
        /// <value>
        /// The point cloud.
        /// </value>
        public CameraSpacePoint[] PointCloud { get; set; }
    }

    /// <summary>
    /// Kinect V2 reader to retrieve Point Clouds.
    /// <para>
    /// Register an Eventhandler, call Initialize() and let it work. Call Stop() to end reading from kinect and
    /// Initialize() to start again.
    /// </para>
    /// </summary>
    public class K4Wv2PointCloud
    {
        #region Fields

        /// <summary>Active Kinect sensor</summary>
        static volatile KinectSensor _kinectSensor = null;
        /// <summary>lock object</summary>
        static object _syncRoot = new Object();
        /// <summary>Reads DepthFrames from Kinect.</summary>
        DepthFrameReader _reader;
        /// <summary>Buffer to hold the depth frame data (16Bit).</summary>
        ushort[] _depthFrameData;
        /// <summary>Counter for frames received</summary>
        int _frameCount;
        /// <summary>Used for max FrameRate</summary>
        Stopwatch _stopWatch;
        /// <summary>Max Frame rate</summary>
        double _frameRate;
        /// <summary>1 / _frameRate</summary>
        long _frameMillis;

        #endregion


        #region Properties & Events

        /// <summary>
        /// Occurs when new Point Cloud is Ready.
        /// </summary>
        public event EventHandler<PointCloudReadyEventArg> PointCloudReady;

        /// <summary>
        /// Gets the (a) KinectSensor Device.
        /// </summary>
        /// <value>
        /// The kinect or null, if no divice found / connected.
        /// </value>
        public KinectSensor Kinect {
            get {
                if (_kinectSensor == null) {
                    lock (_syncRoot) {
                        if (_kinectSensor == null) {
                            // for Alpha, one sensor is supported
                            _kinectSensor = KinectSensor.Default;
                        }
                    }
                }
                return _kinectSensor;
            }
        }

        /// <summary>
        /// Gets or sets the number of skipped frames.<para>
        /// When set, the specified number of depth frames will be skipped, before one point cloud
        /// will be generated.</para>
        /// </summary>
        /// <remarks>
        /// Verwende entweder FrameRate oder SkippedFrames! Ist beides gesetzt, wird FrameRate genommen.
        /// </remarks>
        /// <value>
        /// The number of frames to skip.
        /// </value>
        public int SkippedFrames { get; set; }

        /// <summary>
        /// Gets or sets the maximum frame rate for point clouds (frames per second).
        /// </summary>
        /// <remarks>
        /// Verwende entweder FrameRate oder SkippedFrames! Ist beides gesetzt, wird FrameRate genommen.
        /// </remarks>
        /// <value>
        /// The frame rate. Set to 0 (default), to disable a maximum frame rate.
        /// </value>
        public double FrameRate {
            get {
                return _frameRate;
            }
            set {
                _frameRate = value;
                _frameMillis = (long)((1000.0 / _frameRate) + 0.5);
            }
        }

        /// <summary>
        /// Gets the width of the underlying depth frame.
        /// </summary>
        /// <value>
        /// The width (number of pixels).
        /// </value>
        /// <remarks>
        /// PixelWidth will be actualized by call of <c>K4WPointCloud.Initialize()</c>
        /// </remarks>
        public int PixelWidth { get; private set; }

        /// <summary>
        /// Gets the height of the underlying depth frame.
        /// </summary>
        /// <value>
        /// The height (number of pixels).
        /// </value>
        /// <remarks>
        /// PixelHeight will be actualized by call of <c>K4WPointCloud.Initialize()</c>
        /// </remarks>
        public int PixelHeight { get; private set; }

        ///// <summary>
        ///// Gets the point cloud.
        ///// </summary>
        ///// <value>
        ///// The point cloud.
        ///// </value>
        //public CameraSpacePoint[] PointCloud { get; private set; }

        #endregion


        #region Constructors

        /// <summary>
        /// Initializes a new instance of the <see cref="K4Wv2PointCloud"/> class.
        /// <para>
        /// Register an Eventhandler, call Initialize() and let it work. Call Stop() to end reading from kinect and
        /// Initialize() to start again.
        /// </para>
        /// </summary>
        public K4Wv2PointCloud() {
            //Initialize();
        }

        #endregion

        #region Methods

        public bool Initialize() {
            var kinect = Kinect;
            _stopWatch = new Stopwatch();
            if (kinect != null) {
                kinect.Open();
                // open the reader for the depth frames
                _reader = kinect.DepthFrameSource.OpenReader();
                // allocate space to put the pixels being received and converted
                FrameDescription frameDescription = kinect.DepthFrameSource.FrameDescription;
                _depthFrameData = new ushort[frameDescription.Width * frameDescription.Height];
                // update Width and Height Properties
                PixelWidth = frameDescription.Width;
                PixelHeight = frameDescription.Height;
                // register Event Handler for reading data.
                _reader.FrameArrived += this.Reader_FrameArrived;
                _stopWatch.Start();
                return true;
            }
            else {
                Trace.TraceWarning("Failed to initialize Kinect Sensor.");
                return false;
            }
            
        }

        /// <summary>
        /// Stops the Kinect.
        /// </summary>
        public void Stop() {
            var kinect = Kinect;
            if (_stopWatch != null) {
                _stopWatch.Stop();
                _stopWatch = null;
            }
            if (kinect != null) {
                if (_reader != null) {
                    _reader.FrameArrived -= this.Reader_FrameArrived;
                    _reader.Dispose();
                }
                kinect.Close();
            }
        }

        /// <summary>
        /// Pauses retrieving point clouds.
        /// </summary>
        public void Pause() {
            if (_reader != null) {
                _reader.IsPaused = true;
            }
            if (_stopWatch != null) {
                _stopWatch.Reset();
            }
            _frameCount = 0;
        }

        /// <summary>
        /// Continues retrieving point clouds.
        /// </summary>
        public void Continue() {
            if (_reader != null) {
                _reader.IsPaused = false;
            }
            if (_stopWatch != null) {
                _stopWatch.Restart();
            }
            _frameCount = 0;
        }

        /// <summary>
        /// Handles the FrameArrived event of the Reader control.
        /// </summary>
        /// <param name="sender">The source of the event.</param>
        /// <param name="e">The <see cref="DepthFrameArrivedEventArgs"/> instance containing the event data.</param>
        /// <exception cref="System.NotImplementedException"></exception>
        private void Reader_FrameArrived(object sender, DepthFrameArrivedEventArgs e) {
            // Berücksichtige Max FrameRate, falls gesetzt und verwerfe ggf. Frames
            if (FrameRate > 0) {
                long millis = _stopWatch.ElapsedMilliseconds;
                if (millis < _frameMillis) {
                    return;
                }
                else {
                    _stopWatch.Restart();
                }
            }
            // Zähle Frames und verwerfe die ersten 'SkippedFrames' Stück 
            else {
                _frameCount++;
                if (_frameCount <= SkippedFrames) {
                    return;
                }
                else {
                    _frameCount = 0;
                }
            }

            // Do the work:

            DepthFrameReference frameReference = e.FrameReference;

            try {
                DepthFrame frame = frameReference.AcquireFrame(); // read depth-frame

                if (frame != null) { // maybe referenced frame overwritten already
                    using (frame) {
                        // Copy the depth data from the frame to a temporary array
                        frame.CopyFrameDataToArray(_depthFrameData);
                        // Map the depthPixel data to point cloud
                        var pointCloud = new CameraSpacePoint[_depthFrameData.Length];
                        Kinect.CoordinateMapper.MapDepthFrameToCameraSpace(_depthFrameData, pointCloud);
                        // throw Event
                        if (PointCloudReady != null) {
                            PointCloudReady(this, new PointCloudReadyEventArg { PointCloud = pointCloud });
                        }
                    }
                }
            }
            catch (Exception) {
                Trace.TraceError("Failure at K4Wv2PointCloud.Reader_FrameArrived");
            }
        }

        #endregion
 
    }
}
